/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#include "at32f421.h"
#include "rtthread.h"
#include "motor.h"
#include "rgb.h"

#define M1_ENCODER_REVERSE +1
#define M2_ENCODER_REVERSE -1
struct motor_pos
{
    int32_t cycle;              // 转动圈数
    int32_t encoder : 16;       // 编码器脉冲数
    int32_t angle : 16;         // 角度 0~3600
    int32_t speed : 16;         // 速度 degree/s
    int32_t PulseOneCycle : 16; // 一圈脉冲数
} m1_pos, m2_pos;

static int16_t m1_speed_set = 0;
static int16_t m1_angle_set = 0;
static uint32_t PreMs = 0;
void encoder_init(void)
{
    gpio_init_type gpio_init_struct = {0};
    /* enable tmr3/gpioa clock */
    crm_periph_clock_enable(CRM_TMR3_PERIPH_CLOCK, TRUE);
    crm_periph_clock_enable(CRM_TMR1_PERIPH_CLOCK, TRUE);
    crm_periph_clock_enable(CRM_GPIOA_PERIPH_CLOCK, TRUE);

    /* timer3 output pin Configuration */
    gpio_init_struct.gpio_pins = GPIO_PINS_6 | GPIO_PINS_7 | GPIO_PINS_8 | GPIO_PINS_9;
    gpio_init_struct.gpio_mode = GPIO_MODE_MUX;
    gpio_init_struct.gpio_out_type = GPIO_OUTPUT_PUSH_PULL;
    gpio_init_struct.gpio_pull = GPIO_PULL_NONE;
    gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER;
    gpio_init(GPIOA, &gpio_init_struct);

    gpio_pin_mux_config(GPIOA, GPIO_PINS_SOURCE6, GPIO_MUX_1);
    gpio_pin_mux_config(GPIOA, GPIO_PINS_SOURCE7, GPIO_MUX_1);
    gpio_pin_mux_config(GPIOA, GPIO_PINS_SOURCE8, GPIO_MUX_2);
    gpio_pin_mux_config(GPIOA, GPIO_PINS_SOURCE9, GPIO_MUX_2);

    tmr_base_init(TMR3, 0xFFFF, 0);
    tmr_base_init(TMR1, 0xFFFF, 0);
    tmr_cnt_dir_set(TMR1, TMR_COUNT_UP);
    tmr_cnt_dir_set(TMR3, TMR_COUNT_UP);

    /* config encoder mode */
    tmr_encoder_mode_config(TMR3, TMR_ENCODER_MODE_C, TMR_INPUT_RISING_EDGE, TMR_INPUT_RISING_EDGE);
    tmr_encoder_mode_config(TMR1, TMR_ENCODER_MODE_C, TMR_INPUT_RISING_EDGE, TMR_INPUT_RISING_EDGE);

    /* enable tmr3 */
    tmr_counter_enable(TMR3, TRUE);
    tmr_counter_enable(TMR1, TRUE);
}
void motor_pos_cal(struct motor_pos *mx, int16_t newCounter, uint32_t dt)
{
    // rt_kprintf("new:%4d dt:%4d \n", newCounter, dt);
    // rt_kprintf("cnt:%6d cycle:%4d ", mx->encoder, mx->cycle);

    mx->encoder += newCounter;
    if (mx->encoder > mx->PulseOneCycle)
    {
        mx->cycle++;
        mx->encoder -= mx->PulseOneCycle;
    }

    if (mx->encoder < -mx->PulseOneCycle)
    {
        mx->cycle--;
        mx->encoder += mx->PulseOneCycle;
    }
    // if( mx->encoder < 0)
    // {
    //     mx->cycle--;
    //     mx->encoder += mx->PulseOneCycle;
    // }
    mx->angle = mx->encoder * 3600 / mx->PulseOneCycle;
    mx->speed = newCounter * 360 / mx->PulseOneCycle;
    mx->speed = (int64_t)mx->speed * 1000L / dt;
    // rt_kprintf("ang:%4d spd: %d/dps\n", mx->angle, mx->speed);
}
void encoder_task_init(void)
{
    encoder_init();
    tmr_counter_value_set(TMR1, 0);
    tmr_counter_value_set(TMR3, 0);

    rt_memset(&m1_pos, 0, sizeof(m1_pos));
    rt_memset(&m2_pos, 0, sizeof(m2_pos));
    m1_pos.PulseOneCycle = 574;
    m2_pos.PulseOneCycle = 574;

    m1_speed_pid_init(1.f, 0.5f, 0.01f);
}
void encoder_routine(void)
{
    /**get dt */
    uint32_t CurrentMs = rt_tick_get();
    if(CurrentMs > (PreMs + 20))
    {
        uint32_t dt = CurrentMs - PreMs;
        PreMs = CurrentMs;
        /* get current counter value */
        int16_t counter1 = tmr_counter_value_get(TMR3);
        int16_t counter2 = tmr_counter_value_get(TMR1);

        tmr_counter_value_set(TMR1, 0);
        tmr_counter_value_set(TMR3, 0);

        motor_pos_cal(&m1_pos, counter1, dt);
        motor_pos_cal(&m2_pos, -counter2, dt); // m2 reverse

        // rt_kprintf("m1:%4d/%4d m2:%4d/%4d\n", m1_pos.encoder, m1_pos.cycle, m2_pos.encoder, m2_pos.cycle);
        if(m1_pos.speed != 0 || m2_pos.speed != 0)
        {
            rt_kprintf("speed: %6d, %4d, %6d, ", m1_pos.cycle, m1_pos.angle, m1_pos.speed);
            rt_kprintf("%6d, %4d, %6d \n", m2_pos.cycle, m2_pos.angle, m2_pos.speed);
        }
        // if(m2_pos.speed != 0)
        // {
        // }
    }

    // static float speedLpf = 0;
    // speedLpf = (speedLpf * 0.9f) + (m2_pos.speed * 0.1f);
    // m1_angle_set = m1_pos.angle;

    // int16_t angleErr = m1_angle_set - m1_pos.angle;
    // if (angleErr > 1800)
    // {
    //     angleErr -= 3600;
    // }
    // else if (angleErr < -1800)
    // {
    //     angleErr += 3600;
    // }
    // float speedSet = angleErr;

    // int16_t m1_Out = motor1_pid_control(0, 1000, speedSet, speedLpf, (float)dt * 0.001f);

    // motor2_set_duty(m1_Out);
}
/**
 * @brief  main function.
 * @param  none
 * @retval none
 */
void encoder_task(void *parameter)
{


    while (1)
    {

        // rt_kprintf("M1:%d,%d,%d\n",m1_pos.angle,m1_pos.speed, m1_Out);
        rt_thread_mdelay(20);
    }
}

// #define RT_ENCODER_THREAD_STACK_SIZE 256
// ALIGN(8)
// static rt_uint8_t encoder_stack[RT_ENCODER_THREAD_STACK_SIZE];
// struct rt_thread encoder_thread;
// int encoder_task_init(void)
// {
//     rt_thread_t tid = &encoder_thread;
//     rt_err_t result = rt_thread_init(tid, "encoder", encoder_task, RT_NULL,
//                                      encoder_stack, sizeof(encoder_stack), 10, 20);
//     if (result == RT_EOK)
//     {
//         rt_thread_startup(tid);
//     }
//     else
//     {
//         rt_kprintf("encoder thread init failed\n");
//         return -1;
//     }
//     return 0;
// }
// INIT_APP_EXPORT(encoder_task_init);

#include <stdlib.h>
void motor(int argc, char **argv)
{
    if (argc == 3)
    {
        uint8_t motor_id = atoi(argv[1]);
        int32_t duty = atoi(argv[2]);
        if (motor_id == 1)
        {
            motor1_set_duty(duty);
            // m1_speed_set = duty;
            // m1_angle_set = duty;
        }
        else if (motor_id == 2)
        {
            motor2_set_duty(duty);
        }
        rt_kprintf("motor%d set duty %d\n", motor_id, duty);
    }
    else if (argc == 6)
    {
        uint8_t motor_id = atoi(argv[1]);
        int16_t speed = atoi(argv[2]);
        float kp = atoi(argv[3]);
        float ki = atoi(argv[4]);
        float kd = atoi(argv[5]);
        if (motor_id == 1)
        {
            // m1_speed_set = speed;
            m1_angle_set = speed;
        }
        m1_speed_pid_init(kp * 0.001f, ki * 0.001f, kd * 0.001f);
    }
}
// void rgb(int argc, char **argv)
// {
//     if (argc == 4)
//     {
//         uint8_t r = atoi(argv[1]);
//         uint8_t g = atoi(argv[2]);
//         uint8_t b = atoi(argv[3]);
//         rgb_set(r, g, b);
//         rt_kprintf("rgb set %d %d %d\n", r, g, b);
//     }
// }
void pid(int argc, char **argv)
{
    if (argc == 5)
    {
        uint8_t pidIndex = atoi(argv[1]);
        float kp = atoi(argv[2]);
        float ki = atoi(argv[3]);
        float kd = atoi(argv[4]);
        if (pidIndex == 1)
        {
            m1_speed_pid_init(kp * 0.001f, ki * 0.001f, kd * 0.001f);
        }
    }
}
// MSH_CMD_EXPORT(rgb, rgb set rgb);
MSH_CMD_EXPORT(motor, motor set duty);
